#include "control.h"
#include "tim.h"
#include "main.h"
#include "math.h"
#include "stdio.h"
unsigned int MotorSpeed,MotorSpeed1,MotorSpeed2,MotorSpeed3;  // 全局变量，从编码器中获取电机1~4的编码值
int SpeedTarget,SpeedTarget1,SpeedTarget2,SpeedTarget3;	  // 全局变量,目标电机1~4的控制速度值，执行确定
int MotorOutput,MotorOutpu1,MotorOutput2,MotorOutput3;		      // 全局变量，电机输出的PWM信号值



void GetMotorPulse(void)
{
  // TIM1计数器获得电机脉冲，该电机在20ms采样的脉冲
  MotorSpeed = (short)(__HAL_TIM_GET_COUNTER(&htim3)); 
  __HAL_TIM_SET_COUNTER(&htim3,0);  // 计数器清零

}
void GetMotorPulse1(void)
{
  // TIM2计数器获得电机脉冲，该电机在20ms采样的脉冲
  MotorSpeed1 = (short)(__HAL_TIM_GET_COUNTER(&htim4)); 
  __HAL_TIM_SET_COUNTER(&htim4,0);  // 计数器清零

}
void GetMotorPulse2(void)
{
  // TIM3计数器获得电机脉冲，该电机在20ms采样的脉冲
  MotorSpeed2 = (short)(__HAL_TIM_GET_COUNTER(&htim5)); 
  __HAL_TIM_SET_COUNTER(&htim5,0);  // 计数器清零

}

void GetMotorPulse3(void)
{
  // TIM4计数器获得电机脉冲，该电机在20ms采样的脉冲
  MotorSpeed3 = (short)(__HAL_TIM_GET_COUNTER(&htim8)); 
  __HAL_TIM_SET_COUNTER(&htim8,0);  // 计数器清零

}

/************************增量式PID控制******************************/



int Error_Last,Error_Prev;          // 上一次偏差值，上上次误差
int Pwm_add,Pwm;                    // PWM增量，PWM输出占空比
float Kp =1.2f, Ki = 2.2f, Kd =1.4f;     // PID系数，这里只用到PI控制器

int SpeedInnerControl(int Speed,int Target) // 速度内环控制
{
    int Error = Speed - Target;		  // 偏差 = 目标速度 - 实际速度

    Pwm_add = Kp * (Error - Error_Last) + 										// 比例
							Ki * Error +																		// 积分
							Kd * (Error - 2.0f * Error_Last + Error_Prev)+1;	  // 微分
               // 加一的目的是如果输出信号为0时，系统将进入失控状态

    Pwm += Pwm_add;		              // 原始量+增量 = 输出量
	
		Error_Prev = Error_Last;	      // 保存上上次误差
    Error_Last = Error;	            // 保存上次偏差

  if(Pwm >1000){
		Pwm = 1000;	  }      // 限幅设置最值
  if(Pwm <-1000){
		Pwm = -1000;}

    return Pwm;	                    // 返回输出的PWM值
}


/************************电机1~4的电压和方向控制函数****************************/


void SetMotorVoltageAndDirection(int Pwm)
{
    if(Pwm <= 0)//如果是反转
    {			 
				Pwm = (-Pwm);//如果计算值是负值，先取负得正，因为PWM寄存器只能是正值
         __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1,Pwm);
				__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2,0);//输出信号为负极			
    } 
      else
    {
			
       __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);//输出信号为负极
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, Pwm);
    }
}

void SetMotorVoltageAndDirection1(int Pwm)
{
    if(Pwm <= 0)//如果是反转
    {			 
				Pwm = (-Pwm);//如果计算值是负值，先取负得正，因为PWM寄存器只能是正值
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3,Pwm);
				__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4,0);//输出信号为负极			
    } 
      else
    {
			
       __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0);//输出信号为负极
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, Pwm);
    }
}
void SetMotorVoltageAndDirection2(int Pwm)
{
    if(Pwm <= 0)//如果是反转
    {			 
				Pwm = (-Pwm);//如果计算值是负值，先取负得正，因为PWM寄存器只能是正值
         __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1,Pwm);
				__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2,0);//输出信号为负极			
    } 
      else
    {
			
       __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1,0);//输出信号为负极
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, Pwm);
    }
}
void SetMotorVoltageAndDirection3(int Pwm)
{
    if(Pwm <= 0)//如果是反转
    {			 
				Pwm = (-Pwm);//如果计算值是负值，先取负得正，因为PWM寄存器只能是正值
         __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3,Pwm);
				__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4,0);//输出信号为负极			
    } 
      else
    {
			
       __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, 0);//输出信号为负极
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, Pwm);
    }
}
